// rosbridge_listener.cpp

#include <websocketpp/config/asio_no_tls_client.hpp>
#include <websocketpp/client.hpp>
#include <nlohmann/json.hpp>

#include <iostream>
#include <string>
#include <functional>
#include <thread>
#include <chrono>
#include <atomic>
#include <cstdlib>
#include <signal.h>
#include <iostream>
#include <cmath>
#include <unistd.h>
#include "/home/rm/ros_ws/src/HNURM_UAVDev/hnurm_uav/third_party/rpi_ws281x/ws2811.h"
#include "/home/rm/ros_ws/src/HNURM_UAVDev/hnurm_uav/third_party/rpi_ws281x/clk.h"
#include "/home/rm/ros_ws/src/HNURM_UAVDev/hnurm_uav/third_party/rpi_ws281x/dma.h"
#include "/home/rm/ros_ws/src/HNURM_UAVDev/hnurm_uav/third_party/rpi_ws281x/gpio.h"
#include "/home/rm/ros_ws/src/HNURM_UAVDev/hnurm_uav/third_party/rpi_ws281x/version.h"

#define TARGET_FREQ    WS2811_TARGET_FREQ
#define GPIO_PIN       18  // WS2812 数据引脚
#define DMA            10  // DMA 通道
#define LED_COUNT      30  // LED 灯的数量
enum class led_color{
    RED,
    GREEN,
    PURPLE
};


class LED_show{
    public:
        LED_show(){
             if (ws2811_init(&ledstring) != WS2811_SUCCESS) {
            std::cerr << "ws2811_init failed." << std::endl;
            return ;
        }
        }
        ~LED_show()
        {
            clearStrip();
            ws2811_fini(&ledstring); // 清理资源
        }
        ws2811_t ledstring = {
                .freq = TARGET_FREQ,
                .dmanum = DMA,
                .channel = {
                    [0] = {
                        .gpionum = GPIO_PIN,
                        .invert = 0,
                        .count = LED_COUNT,
                        .strip_type = WS2811_STRIP_GBR, // WS2812B 的色序为 GRB
                        .brightness = 255,
                    },
                },
        };
    public:
        void setColor(int led, uint8_t red, uint8_t green, uint8_t blue)
        {
            ledstring.channel[0].leds[led] = ((uint32_t)green << 16) | ((uint32_t)red << 8) | blue;

        }
        void show(led_color color)
        {
            switch(color){
            case led_color::RED:
                for (int i = 0; i < LED_COUNT; ++i) {
                // 每个 LED 循环设置颜色
                    setColor(   i, 0, 0, 255); // 红色
                    ws2811_render(&ledstring);
                    // usleep(100000); // 延时 100ms
                }
                break;
            case led_color::GREEN:
                for (int i = 0; i < LED_COUNT; ++i) {
                // 每个 LED 循环设置颜色
                    setColor(i, 255, 0, 0); // 绿色
                    ws2811_render(&ledstring);
                    // usleep(100000); // 延时 100ms
                }
                break;
            case led_color::PURPLE:
                for (int i = 0; i < LED_COUNT; ++i) {
                    // 每个 LED 循环设置颜色
                    setColor(i, 0, 255, 255); // 紫色
                    ws2811_render(&ledstring);
                    // usleep(100000); // 延时 100ms
                }
                break;

          }
        }
                void clearStrip()
                {
                    for (int i = 0; i < LED_COUNT; ++i) {
                    setColor(i, 0, 0, 0);
                    }
                    ws2811_render(&ledstring);
                }

};




// 定义 WebSocket++ 客户端类型
typedef websocketpp::client<websocketpp::config::asio_client> client;
typedef websocketpp::config::asio_client::message_type::ptr message_ptr;

// 使用 nlohmann::json
using json = nlohmann::json;

// 原子变量用于标记话题是否有消息发布
std::atomic<bool> camera_odom_active(false);
std::atomic<bool> dji_imu_active(false);

std::atomic<bool> terminate_program(false);
// std::atomic<bool> is_connected(false);


std::atomic<std::uint64_t> last_time_imu{0};
std::atomic<std::uint64_t> last_time_odom{0};
std::chrono::milliseconds TIMEOUT(3000);

// 线程安全队列
template <typename T>
class ThreadSafeQueue {
public:
    void push(T value) {
        std::lock_guard<std::mutex> lock(mtx_);
        queue_.push(std::move(value));
        cv_.notify_one();
    }

    bool pop(T& value) {
        std::unique_lock<std::mutex> lock(mtx_);
        while (queue_.empty()) {
            cv_.wait(lock);
        }
        value = std::move(queue_.back());

        queue_.pop();
        return true;
    }
    void clear() {
        std::lock_guard<std::mutex> lock(mtx_);
        std::queue<T> empty;
        std::swap(queue_, empty);
    }

    int size()
    {
        return queue_.size();
    }


private:
    std::queue<T> queue_;
    std::mutex mtx_;
    std::condition_variable cv_;
};


void signal_handler(int signum){
    std::cout<< "\nInterrupt signal("<<signum <<")received. Terminating program ...."<<std::endl;
    terminate_program.store(true);
}

// std::string create_subscribe_message(const std::string& topic) {
//         json msg;
//         msg["op"] = "subscribe";
//         msg["topic"] = topic;
//         return msg.dump(); }
// 创建订阅消息的函数   必须强制定义对应的类型,让rosbridge能够反复订阅
std::string create_subscribe_message(const std::string &topic,
                                     const std::string &type)
{
    // 构造一个 JSON，比如：
    // {
    //   "op": "subscribe",
    //   "topic": "/camera/odom/sample",
    //   "type": "nav_msgs/Odometry",
    //   "queue_length": 1
    // }
    std::stringstream ss;
    ss << "{"
       << "\"op\":\"subscribe\","
       << "\"topic\":\"" << topic << "\","
       << "\"type\":\"" << type << "\","
       << "\"queue_length\":1"
       << "}";

    return ss.str();
}

ThreadSafeQueue<std::string> message_queue;

// LED更新线程函数
void led_update_thread_func(LED_show& led) {

    while (!terminate_program.load()) {

        std::string msg;
        if (message_queue.pop(msg)) {
            try {
                auto j = json::parse(msg);
                // is_connected.store(true);
                if (j.contains("op") && j["op"] == "publish" && j.contains("topic") && j.contains("msg"))
                // if ( j.contains("topic"))
                 {
                    std::string topic = j["topic"];
                    auto now = std::chrono::system_clock::now();
                    // 转成毫秒自 epoch
                    auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
                    if (j["msg"].is_object() && !j["msg"].empty()) {
                        if (topic == "/camera/odom/sample") {
                            camera_odom_active.store(true);
                            // std::cout << "test" << std::endl;
                            last_time_odom.store(ms, std::memory_order_relaxed);
                        }
                        if (topic == "/dji_sdk/imu") {
                            last_time_imu.store(ms, std::memory_order_relaxed);
                            dji_imu_active.store(true);
                        }
                    }
                    auto last_imu_ms = last_time_imu.load(std::memory_order_relaxed);
                    auto last_odom_ms = last_time_odom.load(std::memory_order_relaxed);
                    auto diff_imu = ms - last_imu_ms;
                    auto diff_odom = ms - last_odom_ms;
                    // 根据标志更新LED
                    if (diff_imu > TIMEOUT.count()) {
                        std::cerr << "[WARNING] IMU data timeout" << std::endl;
                        camera_odom_active.store(false);
                    }
                    if (diff_odom > TIMEOUT.count()) {
                        std::cerr << "[WARNING] Odom data timeout" << std::endl;
                        dji_imu_active.store(false);
                    }

                    if (camera_odom_active.load() && dji_imu_active.load()) {
                        led.show(led_color::GREEN);
                        // std::cout << "GREEN" << std::endl;
                    } else {
                        led.show(led_color::RED);
                        // std::cout << "RED" << std::endl;
                    }
                }
            } catch (json::parse_error& e) {
                std::cerr << "JSON parse error: " << e.what() << std::endl;
            } catch (std::exception& e) {
                std::cerr << "Exception: " << e.what() << std::endl;
            }
        }
    }
    led.clearStrip();
}

int cnt = 0;
void run_client()
{
    LED_show led;
    led.clearStrip();
    std::string uri = "ws://localhost:9090";
    // 定义要订阅的 ROS 话题
    const std::string topic_camera_odom = "/camera/odom/sample";
    const std::string topic_dji_imu = "/dji_sdk/imu";
    try {
            // 创建 WebSocket 客户端实例
            client c;

            // 禁用 WebSocket++ 的日志输出（可选）
            c.clear_access_channels(websocketpp::log::alevel::all);
            c.set_access_channels(websocketpp::log::alevel::connect);
            c.set_access_channels(websocketpp::log::alevel::disconnect);
            c.set_access_channels(websocketpp::log::alevel::app);

            // 初始化 ASIO
            c.init_asio();
            std::thread led_thread(led_update_thread_func, std::ref(led));

            // 设置消息处理函数
            c.set_message_handler([&c,&message_queue, &topic_camera_odom, &topic_dji_imu](websocketpp::connection_hdl hdl, client::message_ptr msg) {
                // 将消息推入队列进行异步处理
                message_queue.push(msg->get_payload());
                // std::cout << "Received message: " << msg->get_payload() << std::endl;
                if(message_queue.size()>1) message_queue.clear();

            });

            // 设置连接打开处理函数
            c.set_open_handler([&c, &topic_camera_odom, &topic_dji_imu](websocketpp::connection_hdl hdl) {
                std::cout << "Connected to rosbridge_server" << std::endl;
                // 发送订阅消息
                websocketpp::lib::error_code ec;
                // std::string subscribe_msg_camera_odom = create_subscribe_message(topic_camera_odom);
                // std::string subscribe_msg_dji_imu = create_subscribe_message(topic_dji_imu);
                std::string subscribe_msg_camera_odom = create_subscribe_message("/camera/odom/sample", "nav_msgs/Odometry");
                std::string subscribe_msg_dji_imu  = create_subscribe_message("/dji_sdk/imu",         "sensor_msgs/Imu");
                c.send(hdl, subscribe_msg_camera_odom, websocketpp::frame::opcode::text);
                std::cout << "Subscribed to topic: " << topic_camera_odom << std::endl;
                c.send(hdl, subscribe_msg_dji_imu, websocketpp::frame::opcode::text);
                std::cout << "Subscribed to topic: " << topic_dji_imu << std::endl;
            });

            // 设置连接失败处理函数
            c.set_fail_handler([&led_thread](websocketpp::connection_hdl hdl) {
                std::cerr << "Connection failed" << std::endl;
                // is_connected.store(false);
                led_thread.join();
                return;
            });

            // 设置连接关闭处理函数
            c.set_close_handler([](websocketpp::connection_hdl hdl) {
                std::cout << "Connection closed" << std::endl;
            });

            // 获取连接句柄
            websocketpp::lib::error_code ec;
            client::connection_ptr con = c.get_connection(uri, ec);
            if (ec) {
                std::cerr << "Could not create connection because: " << ec.message() << std::endl;

                // return ;
            }

            c.connect(con);
            c.run();


            if(terminate_program.load())
            {
                 c.close(con->get_handle(), websocketpp::close::status::normal, "Client closing connection");
                std::cout<<"close client handle"<<std::endl;
                message_queue.push(""); // 以确保线程能退出
                led_thread.join();
            }

        } catch (const websocketpp::exception& e) {
            std::cerr << "WebSocket++ exception: " << e.what() << std::endl;
        } catch (const std::exception& e) {
            std::cerr << "Standard exception: " << e.what() << std::endl;
        } catch (...) {
            std::cerr << "Other exception" << std::endl;
        }

}



int main(int argc, char* argv[]) {

    signal(SIGINT,signal_handler);

    // 默认 ROS Bridge URI




    std::thread client_thread(run_client);
    while(!terminate_program.load()) std::this_thread::sleep_for(std::chrono::milliseconds(10));
    // 关闭客户端线程
    std::cout << "Shutting down WebSocket client..." << std::endl;
    client_thread.join();  // 等待客户端线程退出

    return 0;
}
